/**
 * \file
 * \date 18.09.2013
 * \author alex
 */
#include <bullet_rviz_test/physics.h>
#include <bullet/btBulletDynamicsCommon.h>
#include <bullet/btBulletCollisionCommon.h>

namespace bullet_rviz_test
{

Physics::Physics() :
    last_time_(ros::Time::now())
{
  // Build the broadphase:
  broadphase_ = new btDbvtBroadphase();

  // Set up the collision configuration and dispatcher:
  collision_configuration_ = new btDefaultCollisionConfiguration();
  collision_dispatcher_ = new btCollisionDispatcher(collision_configuration_);

  // The actual physics solver:
  constraint_solver_ = new btSequentialImpulseConstraintSolver();

  // The world:
  world_ = new btDiscreteDynamicsWorld(collision_dispatcher_, broadphase_,
      constraint_solver_, collision_configuration_);
  world_->setGravity(btVector3(0, 0, -9.81));
  //world_->setGravity(btVector3(0, 0, 0));
  world_->getSolverInfo().m_splitImpulse = 1; // Enable split impulse feature (anti-bounce)
}

Physics::~Physics()
{
  delete world_;
  delete constraint_solver_;
  delete collision_dispatcher_;
  delete collision_configuration_;
  delete broadphase_;
}

btDynamicsWorld * Physics::getWorld()
{
  return world_;
}

void Physics::stepSimulation()
{
  ros::Time this_time = ros::Time::now();
  world_->stepSimulation((this_time - last_time_).toSec(), 1000);
  last_time_ = this_time;
}

void Physics::stepSimulation(double time_step)
{
  world_->stepSimulation(time_step, 1000);
}

void Physics::addPhysicalObject(const PhysicalObjectPtr & physical_object)
{
  physical_objects_.push_back(physical_object);
  world_->addRigidBody(physical_object->getRigidBody());
}

void Physics::removePhysicalObject(const PhysicalObjectPtr & physical_object)
{
  world_->removeRigidBody(physical_object->getRigidBody());
  for (PhysicalObjectVector::iterator it = physical_objects_.begin();
      it != physical_objects_.end(); ++it)
  {
    if (it->get() == physical_object.get())
    {
      physical_objects_.erase(it);
      break;
    }
  }
}

}
